﻿// Inner Fire 游戏引擎库
// Contact - 碰撞点
//
// Copyright (c) 2025 by 尔西大帝. All rights reserved.
//
// 所属层级：应用层（4）
// 用于记录碰撞发生后的碰撞点信息。
//
// Author: 尔西大帝
// Email: 2670613871@qq.com
// Created: 2025-07-02

#include "contact.h"
#include "collision.h"
#include <graphics/imath.h>
#include <klog.h>

namespace ifire::game{
  
void Contact::ResolvePenetration(float percent) {
  // 判断合法性
  if (percent < 0.01f) return;
  // 如果都是
  if (a->IsStatic() && b->IsStatic()) return;

  auto v_depth = XMVectorReplicate(depth);
  auto da = v_depth / (a->inv_mass_ + b->inv_mass_) * a->inv_mass_;
  auto db = v_depth / (a->inv_mass_ + b->inv_mass_) * b->inv_mass_;
  auto normal_vec = XMLoadFloat3(&normal);

  // 这里这个检查是因为极端情况下，比如求出无限大的时候，仍然会出现静态物体被移动。
  if (!a->IsStatic()) {
    auto a_pos = XMLoadFloat3(&a->position_);
    XMStoreFloat3(&a->position_, a_pos - normal_vec * da * percent);
    a->MarkBoundingDirty();
  }
  if (!b->IsStatic()) {
    auto b_pos = XMLoadFloat3(&b->position_);
    XMStoreFloat3(&b->position_, b_pos + normal_vec * db * percent);
    b->MarkBoundingDirty();
  }
}

void Contact::ResolveImpulse() {
  // 先需要使用普通的穿透解决把他们分开
  ResolvePenetration();

  // 获得弹力（取最小的）
  float e = imath::Min(a->restitution_, b->restitution_);

  // 获得相对速度
  auto a_velocity = XMLoadFloat3(&a->velocity_);
  auto b_velocity = XMLoadFloat3(&b->velocity_);
  IVec vrel = a_velocity - b_velocity;

  // 应用Impulse公式
  auto e_part = XMVectorReplicate(-(1 + e));
  auto normal_vec = XMLoadFloat3(&normal);
  auto rel_dot_n = XMVector3Dot(vrel, normal_vec );
  auto impulse_mag = e_part * rel_dot_n / (a->inv_mass_ + b->inv_mass_);
  auto jn = normal_vec  * impulse_mag;

  // 分别对项目应用正值和负值
  XMFLOAT3 tmp;
  XMStoreFloat3(&tmp, jn);
  a->ApplyImpulse(tmp);
  XMStoreFloat3(&tmp, -jn);
  b->ApplyImpulse(tmp);
}

void Contact::ResolveImpulseRotation() {
  // ===== 1. 基础准备 =====
  ResolvePenetration();

  // 提取摩擦系数（取平均值）
  float friction = 0.5f * (a->friction_ + b->friction_);

  // ===== 2. 计算法向冲量 =====
  float e = a->restitution_ * b->restitution_;
  auto a_pos = XMLoadFloat3(&a->position_);
  auto b_pos = XMLoadFloat3(&b->position_);
  auto end_vec = XMLoadFloat3(&end);
  auto start_vec = XMLoadFloat3(&start);
  XMVECTOR ra = end_vec - a_pos;
  XMVECTOR rb = start_vec - b_pos;
  auto normal_vec = XMLoadFloat3(&normal);

  // 计算相对速度（含旋转）
  auto a_velocity = XMLoadFloat3(&a->velocity_);
  auto b_velocity = XMLoadFloat3(&b->velocity_);
  auto a_angular = XMLoadFloat3(&a->angular_velocity_);
  auto b_angular = XMLoadFloat3(&b->angular_velocity_);
  auto a_inv_I = XMLoadFloat3x3(&a->GetInvInertiaWorld());
  auto b_inv_I = XMLoadFloat3x3(&b->GetInvInertiaWorld());

  XMVECTOR va = a_velocity + XMVector3Cross(a_angular, ra);
  XMVECTOR vb = b_velocity + XMVector3Cross(b_angular, rb);
  XMVECTOR vrel = va - vb;
  float vrel_mag = XMVectorGetX(XMVector3Length(vrel));

  // 动态衰减恢复系数
  if (vrel_mag > 10.0f) {
    e *= exp(-(vrel_mag - 10.0f) / 5.0f);
  }

  // 碰撞有效性检查
  float vrel_dot_n = XMVectorGetX(XMVector3Dot(vrel, normal_vec));
  if (vrel_dot_n > -1e-5f) return;

  // 法向冲量计算
  XMVECTOR ra_x_n = XMVector3Cross(ra, normal_vec);
  XMVECTOR rb_x_n = XMVector3Cross(rb, normal_vec);

  XMVECTOR inv_ia_ra_x_n = XMVector3Transform(ra_x_n, a_inv_I);
  XMVECTOR inv_ib_rb_x_n = XMVector3Transform(rb_x_n, b_inv_I);

  XMVECTOR angular_part_a = XMVector3Cross(ra, inv_ia_ra_x_n);
  XMVECTOR angular_part_b = XMVector3Cross(rb, inv_ib_rb_x_n);

  float inv_mass_sum = a->inv_mass_ + b->inv_mass_;
  auto angular_dot_n = XMVector3Dot(normal_vec, angular_part_a + angular_part_b);
  float denominator = inv_mass_sum + XMVectorGetX(angular_dot_n);
  if (fabs(denominator) < F_EPSILON) {
    denominator = copysign(F_EPSILON, denominator);
  }

  float jn = -(1.0f + e) * vrel_dot_n / denominator;

  // 法向冲量限制
  float max_j = 0.5f * (a->mass_ + b->mass_) * (vrel_mag + 5.0f);
  jn = imath::Clamp(jn, -max_j, max_j);
  XMVECTOR normal_impulse = normal_vec * jn;

  // ===== 3. 切向摩擦力计算=====
  // 计算切向方向（相对速度在接触面的投影）
  XMVECTOR tangent_velocity = vrel - normal_vec * vrel_dot_n;
  float tangent_len = XMVectorGetX(XMVector3Length(tangent_velocity));

  XMVECTOR friction_impulse = XMVectorZero();
  if (tangent_len > FLT_EPSILON) {
    auto tangent = tangent_velocity / tangent_len;

    // 关键修正2：使用碰撞后的相对速度
    XMVECTOR vrel_after_normal =
        vrel + normal_impulse * (a->inv_mass_ + b->inv_mass_);
    float vrel_dot_t = XMVectorGetX(XMVector3Dot(vrel_after_normal, tangent));

    // 关键修正3：正确的切向分母计算
    XMVECTOR ra_x_t = XMVector3Cross(ra, tangent);
    XMVECTOR rb_x_t = XMVector3Cross(rb, tangent);

    XMVECTOR inv_ia_ra_x_t = XMVector3Transform(ra_x_t, a_inv_I);
    XMVECTOR inv_ib_rb_x_t = XMVector3Transform(rb_x_t, b_inv_I);

    XMVECTOR angular_at = XMVector3Cross(ra, inv_ia_ra_x_t);
    XMVECTOR angular_bt = XMVector3Cross(rb, inv_ib_rb_x_t);
    XMVECTOR angular_dot_t = XMVector3Dot(tangent, angular_at + angular_bt);
    float denominator_t = inv_mass_sum + XMVectorGetX(angular_dot_t);
    if (fabs(denominator_t) < F_EPSILON) {
      denominator_t = copysign(F_EPSILON, denominator_t);
    }
    // 关键修正4：移除恢复系数
    float jt_magnitude = -vrel_dot_t / denominator_t;

    // 静摩擦/动摩擦分离
    float static_friction = 1.2f * friction;
    float dynamic_friction = friction;

    float max_friction;
    if (vrel_mag < 0.1f) { // 低速视为静摩擦
      max_friction = static_friction * abs(jn);
    } else {
      max_friction = dynamic_friction * abs(jn);
    }

    // 库仑摩擦定律
    jt_magnitude = imath::Clamp(jt_magnitude, -max_friction, max_friction);

    friction_impulse = tangent * jt_magnitude;
  }

  // ===== 4. 组合冲量并应用 =====
  XMVECTOR total_impulse = normal_impulse + friction_impulse;

  // 应用冲量（使用正确冲量臂）
  a->ApplyImpulse(StoreF3(total_impulse), StoreF3(ra));
  b->ApplyImpulse(StoreF3(-total_impulse), StoreF3(rb));

  // ===== 5. 调试输出 =====
  // KLOG("Collision: Jn=%.2f, Jt=%.2f, Friction=%.2f, Aω=%.2f, Bω=%.2f",
  //    imath::len(normal_impulse), imath::len(friction_impulse), friction,
  //    imath::len(a->angular_velocity_), imath::len(b->angular_velocity_));
}
}